/********************************************************************************/
/* Projekt  Inverter3                                                             */
/* Autor:   Gerhard Woelfl                                                      */
/* Datum:   30.06.2014                                                          */
/* Changes: 30.06.2014: Initial Version                                         */
/*                                                                              */
/********************************************************************************/

#include <xc.h>
#include "system_const.h"
#include "timer.h"

uint16_t lastTMR2;
uint16_t speedFac;
uint16_t baseAngle;
uint16_t revs;
uint16_t speed;
uint16_t phi;
uint16_t angleInitCnt;
uint16_t t2OvlCnt;
uint16_t angleOffset;


// **** DEBUG values *****************************************
uint16_t latcSim = HALL_POS_0;

#define DEB2_SIZE 16

int16_t deb2ArrA[DEB2_SIZE];
int16_t deb2ArrB[DEB2_SIZE];

uint16_t deb2Idx = 0;

// **** Ende DEBUG values ************************************


// ****************************************************************************
//
//	Function: void timerInit(void)
//
// ****************************************************************************
void timerInit()
{
    T2CONbits.TCKPS = 0b11;     // FOSC/2 = (119,7625MHz/2)/256 = 0,23391Mhz (4,2751uS)
    T2CONbits.TCS = 0;          // Internal clock FCOS/2
    T2CONbits.TSIDL = 1;        // Discontinue timer operation when device enters idle mode
    PR2           = 0xFFFF;     // period register

    IPC1bits.T2IP = 0b100;      // Set Timer 1 Interrupt Priority Level
    IFS0bits.T2IF = 0;          // Clear Timer 1 Interrupt Flag
    IEC0bits.T2IE = 1;          // Enable Timer1 interrupt

    t2OvlCnt      = 0;          // timer 2 overflow counter

    T2CONbits.TON = 1;          // timer on
    lastTMR2      = TMR2;

    angleOffset = ANGLE_OFS;


    switch (HALL_MASK)
    {
        case HALL_POS_0:
            baseAngle = angleOffset; break;
        case HALL_POS_60:
            baseAngle = angleOffset + 120; break;
        case HALL_POS_120:
            baseAngle = angleOffset + 240; break;
        case HALL_POS_180:
            baseAngle = angleOffset + 360; break;
        case HALL_POS_240:
            baseAngle = angleOffset + 480; break;
        case HALL_POS_300:
            baseAngle = angleOffset + 600; break;
        default:
            baseAngle = angleOffset + 0; break;
    }

    phi = baseAngle + 60;

    if (phi >= 720)
    {
        phi -= 720;
    }

    speedFac = 0xFFFF;
    revs = 0;
    speed = 0;

    angleInitCnt = 2;

    return;

}

// ****************************************************************************
//
//	Function: void delay_4_2us(uint16_t delay)
//
// ****************************************************************************
void delay_4_2us(uint16_t delay)
{
    uint16_t startCnt;
    startCnt = TMR2;
    while ((TMR2 - startCnt) < delay);
}

// ****************************************************************************
//
//	Function: void getSpeedVals(uint16_t *speedFac,
//                                  uint16_t *revs,
//                                  uint16_t *speed,
//                                  uint16_t *baseAngle)
//
//       This function is called with each hall event
//
//       The counter t2OvlCnt is reset within this function and
//       incremented with each timer overflow
//
//       When more than one timer overflow occurred between two
//       hall events of when one hall event occurent and the actual
//       TMR2 is greater than the last one, the rotation speed is too
//       slow to determine the correct position; in this case
//       the angle is set to the middle of the sector
//
//       When
//
// ****************************************************************************
void getSpeedVals(void)
{
    uint16_t deltaTMR2;
    uint16_t actualTMR2;
    uint16_t remainder;

    actualTMR2 = TMR2;

    deltaTMR2 = actualTMR2 - lastTMR2;
    lastTMR2 = actualTMR2;

    // determine whether the time between two hall interrupts is greater
    // than 0xFFFF (65535 * 4,275us = 0,280162s
    // Gerhards bike: 51,55 s / rev; speed = 2m/51,55s * 3,6 km/h / (m/s) = 0,14 km/h
    if ((t2OvlCnt > 1) || ((t2OvlCnt > 0) && (TMR2 < lastTMR2)))
    {
        angleInitCnt = 2;
    }
    else
    {
        if (angleInitCnt > 0) angleInitCnt--;
    }

    t2OvlCnt = 0;

    if (angleInitCnt == 0)
    {
        if (deltaTMR2 == 0)
        {
            speedFac = 0x7FFF;
        }
        else
        {
            speedFac = (__builtin_divmodud(ANGLE_DIVIDENT,deltaTMR2,&remainder));
        }
        revs = (uint16_t)(__builtin_muluu(RPM_FAC,speedFac) >> 15);
        speed = (uint16_t)(__builtin_muluu(SPEED_FAC,speedFac) >> 15);
    }
    else
    {
        speedFac = 0xFFFF;
        revs = 0;
        speed = 0;
    }

    switch (HALL_MASK)
    {
        case HALL_POS_0:
            baseAngle = angleOffset; break;
        case HALL_POS_60:
            baseAngle = angleOffset + 120; break;
        case HALL_POS_120:
            baseAngle = angleOffset + 240; break;
        case HALL_POS_180:
            baseAngle = angleOffset + 360; break;
        case HALL_POS_240:
            baseAngle = angleOffset + 480; break;
        case HALL_POS_300:
            baseAngle = angleOffset + 600; break;
        default:
            baseAngle = angleOffset + 0; break;
    }

        // store Monitoring Values
    if (deb2Idx >= (DEB2_SIZE-1))
    {
        deb2Idx = 0;
    }
    else
    {
        deb2Idx++;
    }

    deb2ArrA[deb2Idx] = HALL_MASK;
    deb2ArrB[deb2Idx] = PORTC;

    return;
}

// ****************************************************************************
//
//	Function: uint16_t getAngle(uint16_t angleOfs,
//                                      uint16_t speedFac)
//
// ****************************************************************************
void getAngle(void)
{

    // update phi only after correct initialization
    if (angleInitCnt == 0)
    {
        phi = (uint16_t)(__builtin_muluu((TMR2-lastTMR2),speedFac) >> 14);

        phi += baseAngle;
    }
    else
    {
        // not yet initialized: set phi to middle of sector
        // LATCbits.LATC5 = ~LATCbits.LATC5;
        phi = baseAngle + 60;
    }

    if (phi >= 720)
    {
        phi -= 720;
    }


    return;

}